/**
 *   这里是CAN中断接收函数，接收电机数据,CAN发送函数发送电机电流控制电机.
 **/
#include "Dirver_Can.h"
#include "can.h"
#include "main.h"

static motor_measure_t motor_chassis;

// static CAN_TxHeaderTypeDef gimbal_tx_message;
// static uint8_t gimbal_can_send_data[8];
static CAN_TxHeaderTypeDef chassis_tx_message;
static uint8_t chassis_can_send_data[8];

// motor data read
//  电机数据的读取
#define get_motor_measure(ptr, data)                                           \
  {                                                                            \
    (ptr)->last_ecd = (ptr)->ecd;                                              \
    (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);                       \
    (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);                 \
    (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);             \
    (ptr)->temperate = (data)[6];                                              \
  }

// uint16_t angle_error;          // 角度误差
// uint16_t cyclinder_number = 0; // 转动圈数
//
// void GetRealAngel(motor_measure_t motor_chassis) {
//   motor_chassis.angle = motor_chassis.ecd / 8192.0f * 360.0f; // 电机当前角度
//   angle_error = motor_chassis.ecd - motor_chassis.last_ecd;
//   // 转子的角度差 = 现在转子角度 - 上一次的转子角度
//   if (motor_chassis.speed_rpm > 0) // 如果转子速度为正
//   {
//     if (-18 < cyclinder_number &&
//         cyclinder_number < 18) // 判读输出轴是否转了一圈
//     {
//       if (angle_error < -1000) // 角度为负
//       {
//         cyclinder_number++; // 转子已经转了一圈，cyclinder_number++
//       }
//     } else {
//       cyclinder_number = 0;
//     }
//   } else if (motor_chassis.speed_rpm < 0) {
//     if (-18 < cyclinder_number && cyclinder_number < 18) {
//       if (angle_error > 1000) {
//         cyclinder_number--;
//       }
//     } else {
//       cyclinder_number = 0;
//     }
//   }
//
//   motor_chassis.out_angle =
//       motor_chassis.angle + cyclinder_number * 8191; // 输出轴的机械刻度
//   if (motor_chassis.out_angle >= 0)
//   // 如果这个条件使用，在else中处理一下控制角度的周期
//   {
//     motor_chassis.out_real_angle =
//         motor_chassis.out_angle / (8191 * 18.00f) * 360;
//     // 输出轴的机械刻度换算成角度
//   } else {
//     motor_chassis.out_real_angle =
//         motor_chassis.out_angle / (8191 * 18.00f) * 360;
//     // 输出轴的机械刻度换算成角度
//   }
//   angle_old = motor_chassis.angle;
//   // 记录此时的转子角度，下次接收时作“上一次角度”使用
// }

/*
电机数据：      0:底盘电机1 3508电机,              1:底盘电机2 3508电机,
              2:底盘电机3 3508电机,              3:底盘电机4
3508电机;      4:yaw云台电机 6020电机;            5:pitch云台电机 6020电机;
                6:拨弹电机 2006电机
*/
static uint32_t Total_Round = 0;
static uint32_t Total_Encoder = 0;

uint32_t ConnectFlag = 0;
uint32_t Pre_ConnectFlag = 0;

// hal库CAN回调函数,接收电机数据
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
  ConnectFlag++; // 每收到一次电机通讯，Flag++
  CAN_RxHeaderTypeDef rx_header;
  uint8_t rx_data[8];

  HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);

  get_motor_measure(&motor_chassis, rx_data);

  /****************************************************************************/
  if (motor_chassis.ecd - motor_chassis.last_ecd < -4096)
    Total_Round++;
  else if (motor_chassis.ecd - motor_chassis.last_ecd > 4096)
    Total_Round--;

  Total_Encoder = Total_Round * 8192 + motor_chassis.ecd;

  motor_chassis.angle = (float)Total_Encoder / (float)8192 * 18.0f;
  /****************************************************************************/

  HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
}

/**
 * @brief          发送电机控制电流(0x201,0x202,0x203,0x204)
 * @param[in]      motor1: (0x201) 3508电机控制电流, 范围 [-16384,16384]
 * @param[in]      motor2: (0x202) 3508电机控制电流, 范围 [-16384,16384]
 * @param[in]      motor3: (0x203) 3508电机控制电流, 范围 [-16384,16384]
 * @param[in]      motor4: (0x204) 3508电机控制电流, 范围 [-16384,16384]
 */
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3,
                     int16_t motor4) {
  uint32_t send_mail_box;
  chassis_tx_message.StdId = CAN_CHASSIS_ALL_ID;
  chassis_tx_message.IDE = CAN_ID_STD;
  chassis_tx_message.RTR = CAN_RTR_DATA;
  chassis_tx_message.DLC = 0x08;
  chassis_can_send_data[0] = motor1 >> 8;
  chassis_can_send_data[1] = motor1;
  chassis_can_send_data[2] = motor2 >> 8;
  chassis_can_send_data[3] = motor2;
  chassis_can_send_data[4] = motor3 >> 8;
  chassis_can_send_data[5] = motor3;
  chassis_can_send_data[6] = motor4 >> 8;
  chassis_can_send_data[7] = motor4;

  HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data,
                       &send_mail_box);
}
// 返回 3508电机数据指针
const motor_measure_t *get_chassis_motor_measure_point(uint8_t i) {
  return &motor_chassis;
}

/**
 * CAN滤波器初始化函数
 */
void can_filter_init(void) {
  CAN_FilterTypeDef can_filter_st;
  can_filter_st.FilterActivation = ENABLE;
  can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
  can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
  can_filter_st.FilterIdHigh = 0x0000;
  can_filter_st.FilterIdLow = 0x0000;
  can_filter_st.FilterMaskIdHigh = 0x0000;
  can_filter_st.FilterMaskIdLow = 0x0000;
  can_filter_st.FilterBank = 0;
  can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
  HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
  HAL_CAN_Start(&hcan1);
  HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
}

/**
 * can初始化函数
 * @param hcan
 */
void CAN_Init(CAN_HandleTypeDef *hcan) {
  HAL_CAN_Start(hcan); // 打开can通道
  HAL_CAN_ActivateNotification(hcan,
                               CAN_IT_RX_FIFO0_MSG_PENDING); // 启动CAN接收中断
}

///**
// * @brief          发送电机控制电流(0x205,0x206,0x207,0x208)
// * @param[in]      yaw:   (0x205) 6020电机控制电流, 范围 [-30000,30000]
// * @param[in]      pitch: (0x206) 6020电机控制电流, 范围 [-30000,30000]
// * @param[in]      shoot: (0x207) 2006电机控制电流, 范围 [-10000,10000]
// * @param[in]      rev:   (0x208) 保留，电机控制电流
// */
// void CAN_cmd_gimbal(int16_t yaw, int16_t pitch, int16_t shoot, int16_t rev) {
//  uint32_t send_mail_box;
//  gimbal_tx_message.StdId = CAN_GIMBAL_ALL_ID;
//  gimbal_tx_message.IDE = CAN_ID_STD;
//  gimbal_tx_message.RTR = CAN_RTR_DATA;
//  gimbal_tx_message.DLC = 0x08;
//  gimbal_can_send_data[0] = (yaw >> 8);
//  gimbal_can_send_data[1] = yaw;
//  gimbal_can_send_data[2] = (pitch >> 8);
//  gimbal_can_send_data[3] = pitch;
//  gimbal_can_send_data[4] = (shoot >> 8);
//  gimbal_can_send_data[5] = shoot;
//  gimbal_can_send_data[6] = (rev >> 8);
//  gimbal_can_send_data[7] = rev;
//  HAL_CAN_AddTxMessage(&GIMBAL_CAN, &gimbal_tx_message, gimbal_can_send_data,
//                       &send_mail_box);
//}

//// 发送ID为0x700的CAN包,它会设置3508电机进入快速设置ID
// void CAN_cmd_chassis_reset_ID(void) {
//   uint32_t send_mail_box;
//   chassis_tx_message.StdId = 0x700;
//   chassis_tx_message.IDE = CAN_ID_STD;
//   chassis_tx_message.RTR = CAN_RTR_DATA;
//   chassis_tx_message.DLC = 0x08;
//   chassis_can_send_data[0] = 0;
//   chassis_can_send_data[1] = 0;
//   chassis_can_send_data[2] = 0;
//   chassis_can_send_data[3] = 0;
//   chassis_can_send_data[4] = 0;
//   chassis_can_send_data[5] = 0;
//   chassis_can_send_data[6] = 0;
//   chassis_can_send_data[7] = 0;
//
//   HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message,
//   chassis_can_send_data,
//                        &send_mail_box);
// }

//// 返回 2006电机数据指针
// const motor_measure_t *get_trigger_motor_measure_point(void) {
//   return &motor_chassis[6];
// }

//// 返回yaw 6020电机数据指针
// const motor_measure_t *get_yaw_gimbal_motor_measure_point(void) {
//   return &motor_chassis[4];
// }
//
//// 返回pitch 6020电机数据指针
// const motor_measure_t *get_pitch_gimbal_motor_measure_point(void) {
//   return &motor_chassis[5];
// }

//*******************************************************************新加
